feat: multi level navigation by aclauer · Pull Request #2570 · dimensionalOS/dimos

aclauer · 2026-06-23T19:07:50Z

Problem

3d navigation and ray tracing integration

Closes DIM-XXX

Solution

Creates a blueprint using ray tracing and mls planner, plus improvements to other modules. Click and point to navigate to goals.

Ray tracer:

register point clouds with odometry (breaks on old fastlio recordings!)
outputs region bounds for robust updates

Planner:

incremental builds of planner artifacts in separate task so ingest isn't blocked
path replan on each frame
string pulled paths with tunable cost metrics
levers for path safety

Other things:

add set_motion_mode() to go2 connection, so we can turn off obstacle avoidance and things when instantiating the blueprint
add a simple path follower. Replace this with better trajectory controllers for better results.

How to Test

build native modules:

cd ~/dimos   && ( cd dimos/hardware/sensors/lidar/pointlio/cpp && nix build .#pointlio_native )   && ( cd dimos/mapping/ray_tracing/rust && nix build path:. )   && ( cd dimos/navigation/nav_3d/mls_planner/rust && cargo build --release )

run! (on jetson on go2)

dimos --dtop --rerun-host 0.0.0.0 --rerun-open none run unitree-go2-nav-3d

other things not tested but would be awesome to try:

running with go2 stock lidar
running directly on go2 compute

Contributor License Agreement

I have read and approved the CLA.

codecov · 2026-06-23T19:14:51Z

❌ 1 Tests Failed:

Tests completed	Failed	Passed	Skipped
2279	1	2278	74

View the full list of 1 ❄️ flaky test(s)

dimos.e2e_tests.test_dimsim_spatial_memory::test_go_to_the_bed

Flake rate in main: 23.68% (Passed 29 times, Failed 9 times)

Stack Traces | 563s run time

lcm_spy = <dimos.e2e_tests.lcm_spy.LcmSpy object at 0x74b091c828a0>
start_blueprint = <function start_blueprint.<locals>.set_name_and_start at 0x74b08fda5580>
human_input = <function human_input.<locals>.send_human_input at 0x74b08fda5b20>
dim_sim = <dimos.e2e_tests.dim_sim_client.DimSimClient object at 0x74b092281c10>
explore_house = <function explore_house.<locals>.explore at 0x74b08fda6520>

    @pytest.mark.self_hosted_large
    def test_go_to_the_bed(lcm_spy, start_blueprint, human_input, dim_sim, explore_house) -> None:
        start_blueprint(
            "run",
            "unitree-go2-agentic",
            simulator="dimsim",
        )
        lcm_spy.save_topic(".../McpClient/on_system_modules/res")
        lcm_spy.wait_for_saved_topic(".../McpClient/on_system_modules/res", timeout=1200.0)
    
        explore_house()
    
        human_input("go to the bed")
    
>       lcm_spy.wait_until_odom_position(-3.567, -1.332, threshold=2, timeout=180)

dim_sim    = <dimos.e2e_tests.dim_sim_client.DimSimClient object at 0x74b092281c10>
explore_house = <function explore_house.<locals>.explore at 0x74b08fda6520>
human_input = <function human_input.<locals>.send_human_input at 0x74b08fda5b20>
lcm_spy    = <dimos.e2e_tests.lcm_spy.LcmSpy object at 0x74b091c828a0>
start_blueprint = <function start_blueprint.<locals>.set_name_and_start at 0x74b08fda5580>

dimos/e2e_tests/test_dimsim_spatial_memory.py:32: 
_ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ 
dimos/e2e_tests/lcm_spy.py:182: in wait_until_odom_position
    self.wait_for_message_result(
        predicate  = <function LcmSpy.wait_until_odom_position.<locals>.predicate at 0x74b08fda6700>
        self       = <dimos.e2e_tests.lcm_spy.LcmSpy object at 0x74b091c828a0>
        threshold  = 2
        timeout    = 180
        x          = -3.567
        y          = -1.332
dimos/e2e_tests/lcm_spy.py:168: in wait_for_message_result
    self.wait_until(
        event      = <threading.Event at 0x74b0916a7c50: unset>
        fail_message = 'Failed to get to position x=-3.567, y=-1.332'
        listener   = <function LcmSpy.wait_for_message_result.<locals>.listener at 0x74b08fda5ee0>
        predicate  = <function LcmSpy.wait_until_odom_position.<locals>.predicate at 0x74b08fda6700>
        self       = <dimos.e2e_tests.lcm_spy.LcmSpy object at 0x74b091c828a0>
        timeout    = 180
        topic      = '/odom#geometry_msgs.PoseStamped'
        type       = <class 'dimos.msgs.geometry_msgs.PoseStamped.PoseStamped'>
_ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ 

self = <dimos.e2e_tests.lcm_spy.LcmSpy object at 0x74b091c828a0>

    def wait_until(
        self,
        *,
        condition: Callable[[], bool],
        timeout: float,
        error_message: str,
        poll_interval: float = 0.1,
    ) -> None:
        start_time = time.time()
        while time.time() - start_time < timeout:
            if condition():
                return
            time.sleep(poll_interval)
>       raise TimeoutError(error_message)
E       TimeoutError: Failed to get to position x=-3.567, y=-1.332

condition  = <bound method Event.is_set of <threading.Event at 0x74b0916a7c50: unset>>
error_message = 'Failed to get to position x=-3.567, y=-1.332'
poll_interval = 0.1
self       = <dimos.e2e_tests.lcm_spy.LcmSpy object at 0x74b091c828a0>
start_time = 1782339198.155692
timeout    = 180

dimos/e2e_tests/lcm_spy.py:105: TimeoutError

To view more test analytics, go to the Test Analytics Dashboard
_{📋 Got 3 mins? Take this short survey to help us improve Test Analytics.}

greptile-apps · 2026-06-23T19:16:57Z

Greptile Summary

This PR introduces a full 3D LiDAR-based navigation stack for the Unitree Go2, wiring together point-cloud registration, a ray-tracing voxel map, an MLS surface planner (Rust), and a pure-pursuit path follower.

Rust planner overhaul: NodeId is now CellId (stable across incremental updates), dijkstra_region seeds from cached frontier labels for sub-second re-planning, and string_pull + truncate_to_safe provide smooth, safety-aware waypoint output.
Sensor pipeline: sensor-frame clouds are rotated/translated to world frame before accumulation; batch_local_bounds uses percentile-based cylinder sizing to suppress stray outliers; cylinder bounds are propagated as region_bounds tags into the planner transformer.
Go2 integration: set_motion_mode("mcf") switches the robot to stair-traversal gait; BasicPathFollower runs a background pure-pursuit controller; GoalRelay bridges odometry + clicked goal into planner inputs.

Confidence Score: 5/5

Safe to merge; no P0/P1 issues found across the full diff.

The incremental Dijkstra region update is guarded by live-node filtering, the robot_search radius bug and string_pull baseline bug from the prior review are both confirmed fixed, and the worker/handle split is sound. The only findings are two P2 items (unguarded tag access and non-unit quaternion packing) that do not block correctness of the core navigation stack.

dimos/mapping/ray_tracing/rust/src/main.rs uses a w=0 non-unit quaternion in PoseStamped to carry cylinder bounds — any middleware that normalises the quaternion will silently corrupt the values. dimos/navigation/nav_3d/mls_planner/transformer.py should guard the region_bounds tag access.

Important Files Changed

Filename	Overview
dimos/mapping/ray_tracing/rust/src/voxel_ray_tracer.rs	Added batch_local_bounds (percentile cylinder sizing), parallelised miss-ray casting with rayon, merged pooled_normal+recency into one scan, stack-allocated ArrayVec neighbours.
dimos/mapping/ray_tracing/rust/src/main.rs	Full pose tracking (last_pose), sensor→world transform before update_map, PoseStamped w=0 quaternion repurposed to carry cylinder bounds — fragile to quaternion normalization middleware.
dimos/navigation/nav_3d/mls_planner/rust/src/mls_planner.rs	Incremental update_region / dijkstra_region / build_node_edges_region pipeline; clean public API.
dimos/navigation/nav_3d/mls_planner/rust/src/planner.rs	New plan(), robot_search (base_cost radius guard fixed), string_pull (infinity on infeasible step fixed), truncate_to_safe, follow_preds.
dimos/navigation/nav_3d/mls_planner/rust/src/dijkstra.rs	Weight enum, dijkstra_region with frontier seeding, walk_preds cycle guard via AHashSet.
dimos/navigation/nav_3d/mls_planner/rust/src/edges.rs	NodeId=CellId stable identity, AHashMap node_adj, build_node_edges_region live-node filtering removes stale tombstoned ids.
dimos/navigation/basic_path_follower/module.py	Pure-pursuit follower with dynamic lookahead, background thread, RLock safety, empty-path stop guard.
dimos/mapping/ray_tracing/transformer.py	Sensor-frame→world transform, batch accumulation, region_bounds tag emission.
dimos/navigation/nav_3d/mls_planner/transformer.py	MLSPlan transformer; region_bounds accessed without guard (P2 comment).
dimos/robot/unitree/go2/blueprints/navigation/unitree_go2_nav_3d.py	Full nav pipeline blueprint: PointLio→RayTracingVoxelMap→MLSPlannerNative→GoalRelay+BasicPathFollower.
dimos/robot/unitree/connection.py	set_motion_mode() with status check, conditional switch, and 5 s stabilisation sleep; ReplayConnection stub added.

_{Reviews (10): Last reviewed commit: "Bug fix" | Re-trigger Greptile}

leshy

just one question around AI / normal mode switcher

aclauer

How come this was introduced? generally we never run in "normal" mode - only way we ever used go2 is in sport mode

Yeah it isn't strictly necessary, but probably good to have a way to configure. For example, "AI" mode is actually deprecated as of firmware < 1.1.6 and "MCF" is the new string.

aclauer marked this pull request as ready for review June 23, 2026 19:08

aclauer requested review from arkluc, leshy, mustafab0, paul-nechifor and spomichter as code owners June 23, 2026 19:08

greptile-apps Bot reviewed Jun 23, 2026

View reviewed changes

Comment thread dimos/robot/unitree/connection.py Outdated

Comment thread dimos/navigation/nav_3d/mls_planner/rust/src/planner.rs

Comment thread dimos/navigation/basic_path_follower/module.py

Comment thread dimos/navigation/nav_3d/mls_planner/rust/src/main.rs

greptile-apps Bot reviewed Jun 23, 2026

View reviewed changes

Comment thread dimos/navigation/nav_3d/mls_planner/rust/src/planner.rs Outdated

github-actions Bot added the ready-to-merge Required CI checks have passed on this PR label Jun 23, 2026

aclauer marked this pull request as draft June 23, 2026 21:18

github-actions Bot removed the ready-to-merge Required CI checks have passed on this PR label Jun 23, 2026

aclauer marked this pull request as ready for review June 23, 2026 23:35

github-actions Bot added ready-to-merge Required CI checks have passed on this PR and removed ready-to-merge Required CI checks have passed on this PR labels Jun 24, 2026

leshy reviewed Jun 24, 2026

View reviewed changes

Comment thread dimos/robot/unitree/connection.py

leshy previously approved these changes Jun 24, 2026

View reviewed changes

leshy reviewed Jun 24, 2026

View reviewed changes

Comment thread dimos/robot/unitree/go2/connection.py

aclauer commented Jun 24, 2026

View reviewed changes

aclauer added 7 commits June 24, 2026 12:36

Ray trace local region

4e63c00

Clean up

49c5ab6

Speed improvement

331687f

Incremental planner builds

583e39a

Remove registered flag from transform

c3c7a9a

Teardown and other fixes

1e5e7eb

Remove useless code

a5c7fd4

aclauer added 14 commits June 24, 2026 12:36

Bug fix

1ceef52

Minor fixes

fa5a6b2

Add blueprint

84f7a12

Clean up

601df17

Fix flake

166e160

More planning

c794ffa

Step defaults and voxel size

8ec993c

Validate cached paths on plan fail

cc10edb

Escape the well

1205836

Best effort distance on truncating

75f2335

Bug fix on best effort distance

d8aad1f

Clean up

11d7035

Remove debug logging

4bde883

Remove trivial things

c25585b

aclauer force-pushed the andrew/feat/kronk-nav-4 branch from 02f42cf to c25585b Compare June 24, 2026 19:39

github-actions Bot removed the ready-to-merge Required CI checks have passed on this PR label Jun 24, 2026

paul-nechifor reviewed Jun 24, 2026

View reviewed changes

Comment thread dimos/robot/unitree/connection.py

paul-nechifor reviewed Jun 24, 2026

View reviewed changes

Comment thread dimos/navigation/nav_3d/mls_planner/utils/plan_rrd.py

github-actions Bot added the ready-to-merge Required CI checks have passed on this PR label Jun 24, 2026

Refactor transform main

5e2b87b

aclauer dismissed leshy’s stale review via 5e2b87b June 24, 2026 21:08

github-actions Bot removed the ready-to-merge Required CI checks have passed on this PR label Jun 24, 2026

greptile-apps Bot reviewed Jun 24, 2026

View reviewed changes

Comment thread dimos/navigation/nav_3d/mls_planner/rust/src/edges.rs

github-actions Bot added the ready-to-merge Required CI checks have passed on this PR label Jun 24, 2026

Bug fix

ff34a56

github-actions Bot added ready-to-merge Required CI checks have passed on this PR and removed ready-to-merge Required CI checks have passed on this PR labels Jun 24, 2026

Provide feedback

Saved searches

Use saved searches to filter your results more quickly

Uh oh!

feat: multi level navigation#2570

feat: multi level navigation#2570
aclauer wants to merge 23 commits into
mainfrom
andrew/feat/kronk-nav-4

aclauer commented Jun 23, 2026 •

edited

Loading

Uh oh!

codecov Bot commented Jun 23, 2026 •

edited

Loading

Uh oh!

greptile-apps Bot commented Jun 23, 2026 •

edited

Loading

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

leshy left a comment

Uh oh!

Uh oh!

aclauer left a comment

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Reviewers

Assignees

Labels

Projects

Milestone

Development

Uh oh!

3 participants

Uh oh!

Conversation

aclauer commented Jun 23, 2026 • edited Loading Uh oh! There was an error while loading. Please reload this page.

Uh oh!

Problem

Solution

How to Test

Contributor License Agreement

Uh oh!

codecov Bot commented Jun 23, 2026 • edited Loading Uh oh! There was an error while loading. Please reload this page.

Uh oh!

❌ 1 Tests Failed:

Uh oh!

greptile-apps Bot commented Jun 23, 2026 • edited Loading Uh oh! There was an error while loading. Please reload this page.

Uh oh!

Greptile Summary

Confidence Score: 5/5

Important Files Changed

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

leshy left a comment

Choose a reason for hiding this comment

Uh oh!

Uh oh!

aclauer left a comment

Choose a reason for hiding this comment

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Reviewers

Assignees

Labels

Projects

Milestone

Development

Uh oh!

3 participants

aclauer commented Jun 23, 2026 •

edited

Loading

codecov Bot commented Jun 23, 2026 •

edited

Loading

greptile-apps Bot commented Jun 23, 2026 •

edited

Loading